#include "stm32f10x.h"                  // Device header
#include "OLED.h"
#include "PWN.h"
//#include "ENCODER.h"
#include "TIME.h"
#include "MOTOR.h"
#include "MPU6050.h"
//#include "Serial.h"
#include "Control.h"
#include "MPU6050_Reg.h"
#include "Key.h"

uint8_t KeyNum;

int main()
{
	OLED_Init();
	//OLED_ShowString(1, 3, "Hello world!");
	//Serial_Init();
	PWN_Init_TIM1();
	//Encoder_Init_TIM2and4();
	Time_Init();
	Key_Init();
	//Motor_Init();
	MPU6050_Init();
	//OLED_ShowString(2,10,"woshi");
	while(1)
	{
		KeyNum = Key();
		if(KeyNum == 1) Verical_Kp +=10;
		else if (KeyNum == 2) Verical_Kp -=10;
		/*
		else if (KeyNum == 3) OLED_ShowSignedNum(2,7,KeyNum,1);
		else if (KeyNum == 4) OLED_ShowSignedNum(2,10,KeyNum,1);*/
		else if (KeyNum == 3) Verical_Kd +=3;
		else if (KeyNum == 4) Verical_Kd -=3;
		
		OLED_ShowSignedNum(1,1,A_X,5);
		OLED_ShowSignedNum(1,7,G_Y,5);
		OLED_ShowSignedNum(2,1,G_Z,5);
		OLED_ShowSignedNum(3,1,Speed_Right,5);
		OLED_ShowSignedNum(3,7,Speed_Left,5);
		OLED_ShowSignedNum(4,1,Verical_Kp,4);
		OLED_ShowSignedNum(4,7,Verical_Kd,4);
	}
}


